#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Touch,          sensorTouch)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void turnL (int power, int duration)
{
  motor[motorB]= -1*power;
  motor[motorC]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[motorB]= power;
  motor[motorC]= power;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[motorB]= power;
  motor[motorC]= -1*power;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[motorB]= -1*power;
  motor[motorC]= -1*power;
  wait1Msec(duration);
}

void ProportionalLineFollowing ()
{
	while(SensorValue[Light] >= 0 && SensorValue[Light] <= 20)
	{
		motor[motorB] = 0;
		motor[motorC] = 50;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 20 && SensorValue[Light] <= 40)
	{
		motor[motorB] = 12;
		motor[motorC] = 38;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 40 && SensorValue[Light] <= 60)
	{
		motor[motorB] = 25;
		motor[motorC] = 25;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 60 && SensorValue[Light] <= 80)
	{
		motor[motorB] = 38;
		motor[motorC] = 12;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 80 && SensorValue[Light] <= 100)
	{
		motor[motorB] = 50;
		motor[motorC] = 0;
		wait1Msec(50);
	}
}
task main()
{
	while (SensorValue[Touch] == 0)
	{
		ProportionalLineFollowing();
	}
	if (SensorValue[Touch] == 1)
	{
		Backward(15,1000);
	}
}
